#ifdef KITSCHY_DEBUG_MEMORY 
#include "debug_memorymanager.h"
#endif

#ifdef WIN32
#include "windows.h"
#else
#include "unistd.h"
#endif

#include "stdio.h"
#include "stdlib.h"
#include "string.h"
#include "math.h"
#include "pthread.h"

#include "BList.h"
#include "BVector.h"
#include "geometrics.h"
#include "quaternion.h"
#include "symbol.h"
#include "sort.h"
#include "ontology.h"
#include "featureterm.h"
#include "FTKBase.h"

#include "AWLocalViewport.h"

#include "AWEntity.h"
#include "AWMessage.h"
#include "AWScenario.h"
#include "AWBody.h"
#include "AWSensor.h"
#include "AWBrain.h"
#include "AWTracerBrain.h"
#include "AWThreadedBrain.h"
#include "AWAgent.h"
#include "AWWorld.h"


#include "loader.h"

AW_ThreadedBrain::AW_ThreadedBrain(Ontology *O,FTKBase *Memory,bool pause) : AW_TracerBrain(O,Memory) 
{
	class_name->set("AW_ThreadedBrain");

	m_pause=pause;
//	m_pause_mutex=PTHREAD_MUTEX_INITIALIZER; 
	pthread_mutex_init(&m_pause_mutex,NULL);
//	m_pause_cond=PTHREAD_COND_INITIALIZER; 
	pthread_cond_init(&m_pause_cond, NULL);

//	m_ops_queue_mutex=PTHREAD_MUTEX_INITIALIZER;
	pthread_mutex_init(&m_ops_queue_mutex,NULL);

//	m_sensor_data_mutex=PTHREAD_MUTEX_INITIALIZER;
	pthread_mutex_init(&m_sensor_data_mutex,NULL);

//	m_message_queue_mutex=PTHREAD_MUTEX_INITIALIZER;
	pthread_mutex_init(&m_message_queue_mutex,NULL);

	{
		int retval;
		pthread_attr_init(&m_thread_attr);

		retval=pthread_create(&m_thread, &m_thread_attr, launch_thread, (void *)this);
//		write_debug_report("AW_ThreadedBrain [%p]: thread launch retval: %i\n",this,retval);
	}
} /* AW_ThreadedBrain::AW_ThreadedBrain */ 


AW_ThreadedBrain::AW_ThreadedBrain(FILE *fp,Ontology *O,FTKBase *Memory,bool pause) : AW_TracerBrain(O,Memory)
{
	class_name->set("AW_ThreadedBrain");

	m_pause=pause;
//	m_pause_mutex=PTHREAD_MUTEX_INITIALIZER; 
	pthread_mutex_init(&m_pause_mutex,NULL);
//	m_pause_cond=PTHREAD_COND_INITIALIZER; 
	pthread_cond_init(&m_pause_cond, NULL);

//	m_ops_queue_mutex=PTHREAD_MUTEX_INITIALIZER;
	pthread_mutex_init(&m_ops_queue_mutex,NULL);

//	m_sensor_data_mutex=PTHREAD_MUTEX_INITIALIZER;
	pthread_mutex_init(&m_sensor_data_mutex,NULL);

//	m_message_queue_mutex=PTHREAD_MUTEX_INITIALIZER;
	pthread_mutex_init(&m_message_queue_mutex,NULL);

	load(fp);

	{
		int retval;
		pthread_attr_init(&m_thread_attr);

		retval=pthread_create(&m_thread, &m_thread_attr, launch_thread, (void *)this);
//		write_debug_report("AW_ThreadedBrain [%p]: thread launch retval: %i\n",this,retval);
	}
} /* AW_ThreadedBrain::AW_ThreadedBrain */ 


AW_ThreadedBrain::~AW_ThreadedBrain()
{
	pthread_cancel(m_thread);
	pthread_join(m_thread,0);

	pthread_cond_destroy(&m_pause_cond); 
} /* AW_ThreadedBrain::~AW_ThreadedBrain */ 


bool AW_ThreadedBrain::load(FILE *fp) 
{
	if (!AW_TracerBrain::load(fp)) return false;


	return true;
} /* AW_ThreadedBrain::load */ 


bool AW_ThreadedBrain::save(FILE *fp) 
{
	if (!AW_TracerBrain::save(fp)) return false;


	return true;
} /* AW_ThreadedBrain::save */ 


bool AW_ThreadedBrain::is_a(char *c)
{
	if (strcmp(c,"AW_ThreadedBrain")==0) return true;

	return AW_TracerBrain::is_a(c);
} /* AW_ThreadedBrain::is_a */ 


bool AW_ThreadedBrain::is_a(Symbol *c)
{
	if (c->cmp("AW_ThreadedBrain")) return true;

	return AW_TracerBrain::is_a(c);
} /* AW_ThreadedBrain::is_a */ 


void *AW_ThreadedBrain::launch_thread(void *p)
{
	AW_ThreadedBrain *brain=(AW_ThreadedBrain *)p;

	brain->test_status();
//	write_debug_report("%s [%p]: launch_thread\n",brain->class_name->get(),brain);
	brain->thread();

	return 0;
} /* AW_ThreadedBrain::launch_thread */ 


void AW_ThreadedBrain::pause(void)
{
	pthread_mutex_lock(&m_pause_mutex);
	m_pause=true;
	pthread_mutex_unlock(&m_pause_mutex);
} /* AW_ThreadedBrain::pause */ 


void AW_ThreadedBrain::unpause(void)
{
	pthread_mutex_lock(&m_pause_mutex);
	m_pause=false;
	pthread_mutex_unlock(&m_pause_mutex);
	pthread_cond_broadcast(&m_pause_cond);
//	write_debug_report("AW_ThreadedBrain [%p]: unpause\n",this);
} /* AW_ThreadedBrain::unpause */ 


void AW_ThreadedBrain::actualize(float time,float timeinc,List<AW_Message> *message_queue,
							     Symbol *me,
							     List<FeatureTerm> *sensor_data,	
							     List<FeatureTerm> *posibles,List<FeatureTerm> *l)
{
	AW_Message *m;

	/* Store the sensor data so that the thread can access it: */ 
	pthread_mutex_lock(&m_sensor_data_mutex);
	m_sensor_data.Delete();
	if (sensor_data!=0) {
		m_sensor_data.Copy(*sensor_data);
		sensor_data->Delete();
	} /* if */ 
	pthread_mutex_unlock(&m_sensor_data_mutex);

	message_queue->Rewind();
	while(message_queue->Iterate(m)) process_message(m,time,timeinc,me,sensor_data,posibles,l);
	message_queue->Delete();

	decode_input(time,timeinc,me,sensor_data,posibles,l);
	generate_ops(time,timeinc,me,sensor_data,posibles,l);
} /* AW_ThreadedBrain::actualize */ 


void AW_ThreadedBrain::decode_input(float ,float ,
								 	Symbol *,
									List<FeatureTerm> *,
									List<FeatureTerm> *,
									List<FeatureTerm> *input_ops)
{
	FeatureTerm *op;

	do{
		op=dequeue_operator();
		if (op!=0) {
			input_ops->Add(op);
		} /* if */ 
	}while(op!=0);

} /* AW_ThreadedBrain::decode_input */ 


bool AW_ThreadedBrain::process_message(AW_Message *m,
									 float,float,
								     Symbol *,
									 List<FeatureTerm> *,
								     List<FeatureTerm> *,
								     List<FeatureTerm> *)
{
//	write_debug_report("AW_ThreadedBrain [%s][%p]: message received from %s\n",m->receiver->get(),this,m->sender->get());

	pthread_mutex_lock(&m_message_queue_mutex);
	m_message_queue.Add(new AW_Message(*m));
	pthread_mutex_unlock(&m_message_queue_mutex);
	return true;
} /* AW_ThreadedBrain::process_message */ 


void AW_ThreadedBrain::enqueue_operator(FeatureTerm *op)
{
	pthread_mutex_lock(&m_ops_queue_mutex);
	m_ops_queue.Add(op);
	pthread_mutex_unlock(&m_ops_queue_mutex);
} /* AW_ThreadedBrain::enqueue_operator */ 


FeatureTerm *AW_ThreadedBrain::dequeue_operator(void)
{
	FeatureTerm *op;
	pthread_mutex_lock(&m_ops_queue_mutex);
	if (m_ops_queue.EmptyP()) op=0;
						 else op=m_ops_queue.ExtractIni();
	pthread_mutex_unlock(&m_ops_queue_mutex);
	return op;
} /* AW_ThreadedBrain::dequeue_operator */ 


void AW_ThreadedBrain::thread(void)
{
//	write_debug_report("%s [%p]: default thread routine start\n",class_name->get(),this);
	test_status();
//	write_debug_report("%s [%p]: default thread routine end\n",class_name->get(),this);
} /* AW_ThreadedBrain::thread */ 


void AW_ThreadedBrain::wait(unsigned int milisec)
{
	unsigned int start_time=GetTickCount();

	while(GetTickCount()-start_time<milisec) {
		pthread_mutex_lock(&m_pause_mutex);
		if (m_pause) {
			int tmp_time=GetTickCount();
			while(m_pause) {
				pthread_cond_wait(&m_pause_cond, &m_pause_mutex);
			} /* while */ 
			/* Discount the time that has been paused: */ 
			start_time+=GetTickCount()-tmp_time;
		} /* if */ 
		pthread_mutex_unlock(&m_pause_mutex);
		pthread_testcancel();
#ifdef WIN32
		Sleep(1);
#else
		usleep(1);
#endif
	} /* while */ 
} /* AW_ThreadedBrain::wait */ 



void AW_ThreadedBrain::test_status(void)
{
//	bool stopped=false;
	pthread_mutex_lock(&m_pause_mutex);
	while(m_pause) {
		pthread_cond_wait(&m_pause_cond, &m_pause_mutex);
//		stopped=true;
	} /* while */ 
	pthread_mutex_unlock(&m_pause_mutex);
	pthread_testcancel();

//	if (stopped) write_debug_report("AW_ThreadedBrain [%p]: awaken\n",this);
} /* AW_ThreadedBrain::test_status */ 


bool AW_ThreadedBrain::pending_messages_p(void)
{
	bool res=false;
	pthread_mutex_lock(&m_message_queue_mutex);
	if (m_message_queue.EmptyP()) res=false;
						     else res=true;
	pthread_mutex_unlock(&m_message_queue_mutex);
	return res;
} /* AW_ThreadedBrain::pending_messages_p */ 


void AW_ThreadedBrain::wait_message(void)
{
//	write_debug_report("AW_ThreadedBrain [%p]: waits for messages\n",this);

	while(!pending_messages_p()) {
		test_status();
#ifdef WIN32
		Sleep(1);
#else
		usleep(1);
#endif
	} /* while */ 
} /* AW_ThreadedBrain::wait_message */ 


AW_Message *AW_ThreadedBrain::get_message(void)
{
	AW_Message *m=0;
	pthread_mutex_lock(&m_message_queue_mutex);
	if (m_message_queue.EmptyP()) m=0;
						     else m=m_message_queue.ExtractIni();
	pthread_mutex_unlock(&m_message_queue_mutex);

//	if (m!=0) write_debug_report("AW_ThreadedBrain [%p]: message get from %s\n",this,m->sender->get());

	return m;
} /* AW_ThreadedBrain::get_message */ 

